#include "PID.h"
#include "stm32f10x.h"                  // Device header
#include <math.h>
#include "PhotoelectricSensor.h"
#include "Motor.h"

/**
  * @brief  PID计算，由于各部分需要协调运作，所以要求PID的调控要及时且精准
  * @param  p:指向PID_t结构体的一个变量
  * @retval 无
  */
void PID_Caculate(PID* p)
{
	p->adjustFlag1=1;
	p->adjustFlag2=1;
//	p->adjustFlag3=1;
//	p->adjustFlag4=1;
	
	if(p->adjustFlag1==1)  //通过置标志位来确定是调控哪个电机的速度
	{
		p->Motor1_Error1=p->Motor1_Error0;
		p->Motor1_Error0=p->Target1-p->Actual1;
		
		if(p->Ki!=0)
		{
			p->Motor1_ErrorInt+=p->Motor1_Error0;
		}
		else
		{
			p->Motor1_ErrorInt=0;
		}
		
		/*积分限幅*/
		if(p->Motor1_ErrorInt>500)p->Motor1_ErrorInt=500;
		if(p->Motor1_ErrorInt<-500)p->Motor1_ErrorInt=-500;
		
		/*PID公式Out=Kp*Error0+Ki*ErrorInt+Kd*(Error0-Error1)*/
		p->Out1=p->Kp*p->Motor1_Error0
			  +p->Ki*p->Motor1_ErrorInt
			  +p->Kd*(p->Motor1_Error0-p->Motor1_Error1);
		
		/*输出限幅*/
		if(p->Out1>p->OutMax)p->Out1=p->OutMax;
		if(p->Out1<p->OutMin)p->Out1=p->OutMin;
//		Motor_SetDuty1(p->Out1);
	}
	if(p->adjustFlag2==1)
	{
		p->Motor2_Error1=p->Motor2_Error0;
		p->Motor2_Error0=p->Target2-p->Actual2;
		
		if(p->Ki!=0)
		{
			p->Motor2_ErrorInt+=p->Motor2_Error0;
		}
		else
		{
			p->Motor2_ErrorInt=0;
		}
		if(p->Motor2_ErrorInt>500)p->Motor2_ErrorInt=500;
		if(p->Motor2_ErrorInt<-500)p->Motor2_ErrorInt=-500;
		
		/*PID公式Out=Kp*Motor2_Error0+Ki*Motor2_ErrorInt+Kd*(Motor2_Error0-Motor2_Error1)*/
		p->Out2=p->Kp*p->Motor2_Error0
			  +p->Ki*p->Motor2_ErrorInt
			  +p->Kd*(p->Motor2_Error0-p->Motor2_Error1);
		
		/*输出限幅*/
		if(p->Out2>p->OutMax)p->Out2=p->OutMax;
		if(p->Out2<p->OutMin)p->Out2=p->OutMin;
//		Motor_SetDuty2(p->Out2);
	}
	if(p->adjustFlag3==1)
	{
		p->Motor3_Error1=p->Motor3_Error0;
		p->Motor3_Error0=p->Target3-p->Actual3;
		
		/*变速积分*/
		p->Motor3_ErrorInt+=p->Motor3_Error0;
		
		if(p->Motor3_ErrorInt>500)p->Motor3_ErrorInt=500;
		if(p->Motor3_ErrorInt<-500)p->Motor3_ErrorInt=-500;
		
		/*PID公式Out=Kp*Motor3_Error0+Ki*Motor3_ErrorInt+Kd*(Motor3_Error0-Motor3_Error1)*/
		p->Out3=p->Kp*p->Motor3_Error0
			  +p->Ki*p->Motor3_ErrorInt
			  +p->Kd*(p->Motor3_Error0-p->Motor3_Error1);
		
		/*输出限幅*/
		if(p->Out3>p->OutMax)p->Out3=p->OutMax;
		if(p->Out3<p->OutMin)p->Out3=p->OutMin;
//		Motor_SetDuty3(p->Out3);		
	}
	if(p->adjustFlag4==1)
	{
		p->Motor4_Error1=p->Motor4_Error0;
		p->Motor4_Error0=p->Target4-p->Actual4;
		
		p->Motor4_ErrorInt+=p->Motor4_Error0;
		
		if(p->Motor4_ErrorInt>500)p->Motor4_ErrorInt=500;
		if(p->Motor4_ErrorInt<-500)p->Motor4_ErrorInt=-500;
		
		/*PID公式Out=Kp*Motor4_Error0+Ki*Motor4_ErrorInt+Kd*(Motor4_Error0-Motor4_Error1)*/
		p->Out4=p->Kp*p->Motor4_Error0
			  +p->Ki*p->Motor4_ErrorInt
			  +p->Kd*(p->Motor4_Error0-p->Motor4_Error1);
		
		/*输出限幅*/
		if(p->Out4>p->OutMax)p->Out4=p->OutMax;
		if(p->Out4<p->OutMin)p->Out4=p->OutMin;
//		Motor_SetDuty4(p->Out4);		
	}
	p->adjustFlag1=0;
	p->adjustFlag2=0;
	p->adjustFlag3=0;
	p->adjustFlag4=0;
}
